function q = ikine2(T_desired, q0, DH_para, q4_new)
ox_desired = T_desired(1, 2); oy_desired = T_desired(2, 2); oz_desired = T_desired(3, 2);
ax_desired = T_desired(1, 3); ay_desired = T_desired(2, 3); az_desired = T_desired(3, 3);

o_desired = [ox_desired; oy_desired; oz_desired];
a_desired = [ax_desired; ay_desired; az_desired];

q4 = q4_new;

q1 = q0(1); q2 = q0(2); q3 = q0(3); q4 = q4; q5 = q0(5); q6 = q0(6);
q = [q1, q2, q3, q4, q5, q6];
F = calculate_f(q, o_desired, a_desired);
Je = calculate_Je(q, o_desired, a_desired);
thetak = [q5; q6];
for i = 1:10
    thetak = thetak - Je * F;
    q = [q1, q2, q3, q4, thetak(1), thetak(2)];
    F = calculate_f(q, o_desired, a_desired);
    Je = calculate_Je(q, o_desired, a_desired);
end
q5 = thetak(1);
q6 = thetak(2);

Alpha = DH_para.Alpha;
A = DH_para.A;
D = DH_para.D;
Theta = DH_para.Theta;
Sigma = DH_para.Sigma;

T34 = transformation_mdh(Alpha(4), A(4), D(4), Theta(4), Sigma(4), q4);
T45 = transformation_mdh(Alpha(5), A(5), D(5), Theta(5), Sigma(5), q5);
T56 = transformation_mdh(Alpha(6), A(6), D(6), Theta(6), Sigma(6), q6);
T36 = T34 * T45 * T56;

px36 = T36(1, 4); py36 = T36(2, 4); pz36 = T36(3, 4);

a2 = A(2); a3 = A(3); a4 = A(4)*0 + px36;
d1 = D(1); d3 = D(3) + pz36; d4 = D(4)*0 + norm(py36); d6 = D(6);

Px = T_desired(1, 4); Py = T_desired(2, 4); Pz = T_desired(3, 4) - d1;
theta0 = q0 + Theta;

theta1_1 = atan2(d3, sqrt(Px^2 + Py^2 - d3^2)) + atan2(Py, Px);
theta1_2 = atan2(d3, -sqrt(Px^2 + Py^2 - d3^2)) + atan2(Py, Px);

if abs(theta1_1 - theta0(1)) <= abs(theta1_2 - theta0(1))
    theta1 = theta1_1;
else
    theta1 = theta1_2;
end

k3 = ((a2 - cos(theta1) * Px - sin(theta1) * Py)^2 + Pz^2 - (a4^2 + d4^2 + a3^2)) / (2 * a3);
theta3_1 = atan2(k3, sqrt(a4^2 + d4^2 - k3^2)) - atan2(a4, d4);
theta3_2 = atan2(k3, -sqrt(a4^2 + d4^2 - k3^2)) - atan2(a4, d4);
if abs(theta3_1 - theta0(3)) <= abs(theta3_2 - theta0(3))
    theta3 = theta3_1;
else
    theta3 = theta3_2;
end

g = cos(theta1) * Px + sin(theta1) * Py - a2;
e = a4 * cos(theta3) + d4 * sin(theta3) + a3;
f = a4 * sin(theta3) - d4 * cos(theta3);
if g^2 + Pz^2 > 0
    theta2 = atan2(Pz * e - g * f, g * e + Pz * f);
else
    theta2 = pi / 2;
end
q1 = theta1 - Theta(1);
q2 = theta2 - Theta(2);
q3 = theta3 - Theta(3);

q = [q1, q2, q3, q4, q5, q6];